#pragma once
#include "elisa/elisa.h"
#include <iostream>
#include <sstream>
#include <algorithm>
#include <string>
#include <vector>
#include <gaol/gaol>

#include "Modeler.h"

#ifdef ELISA_PERFORMER
#include "elisa_chrono.h"
#endif

using namespace std;
using namespace elisa;

/* Problem: robot kinematics problem

Anne-Mercedes Bellido:
  "Construction of iteration functions for the simultaneous computation
   of the solutions of equations and algebraic systems"
  Numerical Algorithms Vo. 6, Nos 3-4, pages 317--351, 1992.
*/
class Kinematics3 : public Modeler
{
private:
	Model _model;

	RealVar z1;
	RealVar z2;
	RealVar z3;
	RealVar z4;
	RealVar z5;
	RealVar z6;
	RealVar z7;
	RealVar z8;
	RealVar z9;
	RealConstraint c1;
	RealConstraint c2;
	RealConstraint c3;
	RealConstraint c4;
	RealConstraint c5;
	RealConstraint c6;
	RealConstraint c7;
	RealConstraint c8;
	RealConstraint c9;

public:
	Kinematics3()
		: z1(Interval(-1.0e8,1.0e8), "z1"),
		  z2(Interval(-1.0e8,1.0e8), "z2"),
		  z3(Interval(-1.0e8,1.0e8), "z3"),
		  z4(Interval(-1.0e8,1.0e8), "z4"),
		  z5(Interval(-1.0e8,1.0e8), "z5"),
		  z6(Interval(-1.0e8,1.0e8), "z6"),
		  z7(Interval(-1.0e8,1.0e8), "z7"),
		  z8(Interval(-1.0e8,1.0e8), "z8"),
		  z9(Interval(-1.0e8,1.0e8), "z9"),
		  c1(pow(z1,2) + pow(z2,2) + pow(z3,2) - 12*z1 == 68),
		  c2(pow(z4,2) + pow(z5,2) + pow(z6,2) - 12*z5 == 68),
		  c3(pow(z7,2) + pow(z8,2) + pow(z9,2) - 24*z8 - 12*z9 == -100),
		  c4(z1*z4 + z2*z5 + z3*z6 - 6*z1 - 6*z5 == 52),
		  c5(z1*z7 + z2*z8 + z3*z9 - 6*z1 - 12*z8 - 6*z9 == -64),
		  c6(z4*z7 + z5*z8 + z6*z9 - 6*z5 - 12*z8 - 6*z9 == -32),
		  c7(2*z2 + 2*z3 - z4 - z5 - 2*z6 - z7 - z9 == -18),
		  c8(z1 + z2 + 2*z3 + 2*z4 + 2*z6 - 2*z7 + z8 - z9 == 38),
		  c9(z1 + z3 - 2*z4 + z5 - z6 + 2*z7 - 2*z8 == -8),
		  _model()
	{
	}

	elisa::Model & model()
	{
		_model = c1 && c2 && c3 && c4 && c5 && c6 && c7 && c8 && c9;
		return _model;
	}

	void print()
	{
		cout << '\t' << z1.getName() << " = " << z1.domain() << endl;
		cout << '\t' << z2.getName() << " = " << z2.domain() << endl;
		cout << '\t' << z3.getName() << " = " << z3.domain() << endl;
		cout << '\t' << z4.getName() << " = " << z4.domain() << endl;
		cout << '\t' << z5.getName() << " = " << z5.domain() << endl;
		cout << '\t' << z6.getName() << " = " << z6.domain() << endl;
		cout << '\t' << z7.getName() << " = " << z7.domain() << endl;
		cout << '\t' << z8.getName() << " = " << z8.domain() << endl;
		cout << '\t' << z9.getName() << " = " << z9.domain() << endl;
	}

	RealVar & varX()
	{
		return z1;
	}
	RealVar & varY()
	{
		return z2;
	}
};
